#ifndef CUSTOM_CONTROL_H
#define CUSTOM_CONTROL_H

#ifdef __cplusplus
extern "C" {
#endif

#define MAX_DATA_SIZE 256 // bytes

typedef struct {
  double Kp;
  double Ki;
  double Kd;
  unsigned char *data;
} PidHandle;

typedef struct {
  double K;
  double T1;
  double T2;
  double tau;
  unsigned char* data;
} DobHandle;

typedef struct {
  double K[10];
  double tau1[10];
  double tau2[10];
  unsigned char* data;
} LeadLagHandle;

typedef struct {
  double prev_u;
  double prev_y;
} TF;

double custom_pid_control(double r, double y, double dt, PidHandle handle);
double custom_dob_control(double u, double y, double dt, DobHandle handle);
double custom_lead_lag_control(double r, double y, double dt, LeadLagHandle handle);
double custom_dual_pid_control(double r_pos, double* r_vel, double y_vel, double y_pos, double dt, PidHandle outer_handle, PidHandle inner_handle);

void custom_pid_reset(PidHandle handle);
void custom_dob_reset(DobHandle handle);
void custom_lead_lag_reset(LeadLagHandle handle);
void custom_dual_pid_reset(PidHandle outer_handle, PidHandle inner_handle);

#ifdef __cplusplus
}
#endif

#endif
